﻿import smbus2,time
import ctypes

from Quaternion import Quaternion as Q
from Quaternion import XYZVector as V

class dmp_mpu6050:

    __DMP_packet_size = 0
    
    #内置参数
    
    MPU6050_ADDR = 0x68 #地址
    PWR_MGMT_1   = 0x6B
    SMPLRT_DIV   = 0x19
    CONFIG       = 0x1A
    GYRO_CONFIG  = 0x1B
    ACCEL_CONFIG = 0x1C
    INT_ENABLE   = 0x38
    ACCEL_XOUT_H = 0x3B#加速度
    ACCEL_YOUT_H = 0x3D
    ACCEL_ZOUT_H = 0x3F
    TEMP_OUT_H   = 0x41#温度
    GYRO_XOUT_H  = 0x43#角速度
    GYRO_YOUT_H  = 0x45
    GYRO_ZOUT_H  = 0x47


    #测量参数
    X_A_OFF = int(-1581.625)
    Y_A_OFF = int(1510.5)
    Z_A_OFF = int(-778.875)
    X_G_OFF = int(16.75)
    Y_G_OFF = int(-14.75)
    Z_G_OFF = int(-21.0)
    
    #偏移参数地址
    X_A_OFF_ADD_1 = 0x06
    X_A_OFF_ADD_2 = 0x07
    Y_A_OFF_ADD_1 = 0x08
    Y_A_OFF_ADD_2 = 0x09
    Z_A_OFF_ADD_1 = 0x08
    Z_A_OFF_ADD_2 = 0x0B
    X_G_OFF_ADD_1 = 0x13
    X_G_OFF_ADD_2 = 0x14
    Y_G_OFF_ADD_1 = 0x15
    Y_G_OFF_ADD_2 = 0x16
    Z_G_OFF_ADD_1 = 0x17
    Z_G_OFF_ADD_2 = 0x18

    # dmpMemory has size MPU6050_DMP_CODE_SIZE = 1929
    dmpMemory = [
        # bank 0, 256 bytes
        0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02,
        0x00, 0x03, 0x00, 0x00,
        0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B,
        0x12, 0x82, 0x00, 0x01,
        0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF,
        0xFF, 0xFE, 0x80, 0x01,
        0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09,
        0x3E, 0x80, 0x00, 0x00,
        0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
        0x60, 0x00, 0x00, 0x00,
        0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55,
        0x00, 0x00, 0x21, 0x82,
        0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00,
        0x00, 0x05, 0x80, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
        0x00, 0x03, 0x00, 0x00,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32,
        0x00, 0x00, 0x5E, 0xC0,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B,
        0x00, 0x6C, 0x12, 0xCC,
        0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00,
        0x40, 0x00, 0x01, 0xF4,
        0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6,
        0x00, 0x00, 0x27, 0x10,

        # bank 1, 256 bytes
        0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00,
        0x01, 0x00, 0x00, 0x00,
        0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0,
        0xFF, 0xD9, 0x5B, 0xC8,
        0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2,
        0x00, 0xCE, 0xBB, 0xF7,
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02,
        0x02, 0x00, 0x00, 0x0C,
        0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00,
        0x40, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00,
        0x00, 0x00, 0x00, 0x14,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC,
        0xC6, 0x7E, 0xD1, 0x6C,
        0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0,
        0x00, 0x00, 0x00, 0x30,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE,
        0x00, 0x0C, 0x02, 0xD0,

        # bank 2, 256 bytes
        0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00,
        0x00, 0x00, 0x01, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00,
        0xFF, 0xEF, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x40, 0x00, 0x00, 0x00,
        0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,

        # bank 3, 256 bytes
        0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91,
        0xF7, 0x4A, 0x90, 0x7F,
        0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2,
        0xCE, 0x81, 0xF3, 0xC2,
        0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8,
        0x80, 0xBA, 0xA7, 0xDF,
        0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94,
        0x24, 0x48, 0x70, 0x3C,
        0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D,
        0x55, 0x7D, 0xD8, 0xB1,
        0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C,
        0x80, 0xA8, 0xF1, 0x01,
        0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2,
        0x0E, 0xB8, 0x97, 0x80,
        0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5,
        0xCD, 0xC7, 0xA9, 0x0C,
        0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66,
        0xB0, 0xB4, 0xBA, 0x80,
        0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00,
        0x89, 0x0E, 0x16, 0x1E,
        0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36,
        0x56, 0x76, 0xF1, 0xB9,
        0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98,
        0xB9, 0xAF, 0xF0, 0x24,
        0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83,
        0xB5, 0x93, 0xAF, 0xF0,
        0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF,
        0xD9, 0xFA, 0xA3, 0x86,
        0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5,
        0xC7, 0xB2, 0x8C, 0xC1,
        0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1,
        0xB8, 0xA8, 0xB2, 0x86,

        # bank 4, 256 bytes
        0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35,
        0x3D, 0xB2, 0xB6, 0xBA,
        0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A,
        0xB8, 0xAA, 0x87, 0x2C,
        0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4,
        0xCD, 0xC9, 0xF1, 0xB8,
        0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55,
        0x7D, 0xB5, 0x93, 0xA3,
        0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97,
        0x83, 0xA8, 0x11, 0x84,
        0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64,
        0x18, 0xD8, 0xF1, 0xA5,
        0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A,
        0x40, 0x48, 0xF9, 0xF3,
        0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18,
        0x97, 0x82, 0xA8, 0xF1,
        0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3,
        0xDE, 0xD8, 0x83, 0xA5,
        0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1,
        0x84, 0x92, 0xA2, 0x4D,
        0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32,
        0xD8, 0x50, 0x71, 0xD9,
        0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8,
        0x78, 0x93, 0xA3, 0x4D,
        0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32,
        0xD8, 0x50, 0x71, 0xD9,
        0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8,
        0x78, 0xA8, 0x8A, 0x9A,
        0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98,
        0xA8, 0xD9, 0x08, 0xD8,
        0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1,
        0xAF, 0xC8, 0x97, 0x87,

        # bank 5, 256 bytes
        0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D,
        0xF3, 0xD9, 0x2A, 0xD8,
        0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9,
        0x61, 0xD8, 0x6C, 0x68,
        0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3,
        0x84, 0x19, 0x3D, 0x5D,
        0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8,
        0xB0, 0xAF, 0x8F, 0x94,
        0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8,
        0x87, 0x9A, 0x35, 0xDA,
        0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98,
        0x85, 0x02, 0x2E, 0x56,
        0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9,
        0x28, 0xD8, 0x4D, 0xD9,
        0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8,
        0xA3, 0x29, 0x83, 0xDA,
        0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8,
        0xB8, 0xB0, 0xA8, 0x8A,
        0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA,
        0xDE, 0xD8, 0xA8, 0x60,
        0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06,
        0x99, 0x07, 0xAB, 0x97,
        0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8,
        0x8C, 0x9C, 0xF0, 0x04,
        0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98,
        0x2C, 0x50, 0x50, 0x78,
        0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79,
        0x8B, 0x29, 0x51, 0x79,
        0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38,
        0x8B, 0x39, 0x40, 0x68,
        0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09,
        0x71, 0x58, 0x44, 0x68,

        # bank 6, 256 bytes
        0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C,
        0xF0, 0x8C, 0xA8, 0x04,
        0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C,
        0x02, 0x26, 0x46, 0x66,
        0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69,
        0x38, 0x64, 0x48, 0x31,
        0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8,
        0x19, 0x31, 0x48, 0x60,
        0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19,
        0x86, 0xA8, 0x6E, 0x76,
        0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22,
        0x8A, 0x6E, 0x8A, 0x56,
        0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E,
        0x6E, 0x9D, 0xB8, 0xAD,
        0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D,
        0x55, 0x7D, 0x81, 0x91,
        0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8,
        0x4D, 0xD9, 0x48, 0xD8,
        0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8,
        0x51, 0xD9, 0x04, 0xAE,
        0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE,
        0x19, 0x81, 0xAD, 0xD9,
        0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7,
        0xD9, 0xAD, 0xAD, 0xAD,
        0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E,
        0x76, 0xF3, 0xAC, 0x2E,
        0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C,
        0xAC, 0x30, 0x18, 0xA8,
        0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C,
        0x24, 0xF2, 0xB0, 0x89,
        0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51,
        0xD9, 0xD8, 0xD8, 0x79,

        # bank 7, 138 bytes (remainder)
        0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91,
        0x2D, 0xD9, 0x28, 0xD8,
        0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35,
        0x3D, 0x80, 0x25, 0xDA,
        0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28,
        0x34, 0x3C, 0xF3, 0xAB,
        0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0,
        0x87, 0x9C, 0xB9, 0xA3,
        0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D,
        0xF1, 0xA3, 0xA3, 0xA3,
        0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
        0xA3, 0xA3, 0xA3, 0xA3,
        0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99,
        0xF1, 0xA3, 0xA3, 0xA3,
        0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3,
        0x9B, 0xA3, 0xA3, 0xDC,
        0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF]

    # dmpConfig has size MPU6050_DMP_CONFIG_SIZE = 192
    dmpConfig = [
        #  BANK    OFFSET  LENGTH  [DATA]
        0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C,  # FCFG_1 inv_set_gyro_calibration
        0x03, 0xAB, 0x03, 0x36, 0x56, 0x76,  # FCFG_3 inv_set_gyro_calibration
        0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2,
        # D_0_104 inv_set_gyro_calibration
        0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1,
        # D_0_24 inv_set_gyro_calibration
        0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00,
        # D_1_152 inv_set_accel_calibration
        0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
        # FCFG_2 inv_set_accel_calibration
        0x03, 0x89, 0x03, 0x26, 0x46, 0x66,  # FCFG_7 inv_set_accel_calibration
        0x00, 0x6C, 0x02, 0x20, 0x00,  # D_0_108 inv_set_accel_calibration
        0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00,
        # CPASS_MTX_00 inv_set_compass_calibration
        0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_01
        0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_02
        0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_10
        0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_11
        0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_12
        0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_20
        0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_21
        0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00,  # CPASS_MTX_22
        0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00,
        # D_1_236 inv_apply_endian_accel
        0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
        # FCFG_2 inv_set_mpu_sensors
        0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D,
        # CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
        0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D,  # FCFG_5 inv_set_bias_update
        0x00, 0xA3, 0x01, 0x00,  # D_0_163 inv_set_dead_zone
        # SPECIAL 0x01 = enable interrupts
        0x00, 0x00, 0x00, 0x01,  # SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
        0x07, 0x86, 0x01, 0xFE,  # CFG_6 inv_set_fifo_interupt
        0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38,
        # CFG_8 inv_send_quaternion
        0x07, 0x7E, 0x01, 0x30,  # CFG_16 inv_set_footer
        0x07, 0x46, 0x01, 0x9A,  # CFG_GYRO_SOURCE inv_send_gyro
        0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38,
        # CFG_9 inv_send_gyro -> inv_construct3_fifo
        0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38,
        # CFG_12 inv_send_accel -> inv_construct3_fifo
        0x02, 0x16, 0x02, 0x00, 0x05]  # D_0_22 inv_set_fifo_rate

        # dmpUpdates has size MPU6050_DMP_UPDATES_SIZE = 47
    dmpUpdates = [
        0x01, 0xB2, 0x02, 0xFF, 0xFF,
        0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
        0x01, 0x6A, 0x02, 0x06, 0x00,
        0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
        0x01, 0x62, 0x02, 0x00, 0x00,
        0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00]

    def __init__(self):
        i2c_bus = 0
        self.bus = smbus2.SMBus(i2c_bus)
        #频率  sample rate = 8 kHz/(1+samp_rate_div)  
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.SMPLRT_DIV, 4)
        # time.sleep(0.1)
        # #工作方式设置---比较复杂
        # #重置传感器
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR,dmp_mpu6050.PWR_MGMT_1,0x00)
        # time.sleep(0.1)
        # #电源和时钟
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.PWR_MGMT_1, 0x01)
        # time.sleep(0.1)
        # #配置寄存器
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.CONFIG, 0)
        # time.sleep(0.1)
        #测量区间
        
        #加速度区间写入
        # gyro_config_sel = [0b00000,0b010000,0b10000,0b11000] # byte registers
        # gyro_config_vals = [250.0,500.0,1000.0,2000.0] # degrees/sec
        # gyro_index = 0
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.GYRO_CONFIG, int(gyro_config_sel[gyro_index]))
        # time.sleep(0.1)
        # #角速度写入
        # accel_config_sel = [0b00000,0b01000,0b10000,0b11000] # byte registers
        # accel_config_vals = [2.0,4.0,8.0,16.0] # g (g = 9.81 m/s^2)
        # accel_index = 0
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.ACCEL_CONFIG, int(accel_config_sel[accel_index]))
        # time.sleep(0.1)
        # #FIFO 相关写入
        # self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.INT_ENABLE, 1)
        # time.sleep(0.1)
        
        #使用另一个的初始化方式
        self.write_bits(dmp_mpu6050.PWR_MGMT_1, 2,
                        3, 0x01)
        
        self.write_bits(0x1C,
                        4,
                        2, 0x00)

        self.write_bits(0x1B,
                        4,
                        2, 0x00)
        self.write_bit(
            dmp_mpu6050.PWR_MGMT_1, 6, 0)
        
        
        
        
        
        
        
        #偏差写入
        self.set_x_accel_offset(dmp_mpu6050.X_A_OFF)
        self.set_y_accel_offset(dmp_mpu6050.Y_A_OFF)
        self.set_z_accel_offset(dmp_mpu6050.Z_A_OFF)
        self.set_x_gyro_offset(dmp_mpu6050.X_G_OFF)
        self.set_y_gyro_offset(dmp_mpu6050.Y_G_OFF)
        self.set_z_gyro_offset(dmp_mpu6050.Z_G_OFF)
        
        #测量标记值  用于原始参数转换成需要的参数
        # self.GYRO_CONFIG = gyro_config_vals[gyro_index]
        # self.ACCEL_CONFIG = accel_config_vals[accel_index]


    def set_x_accel_offset(self,a_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.X_A_OFF_ADD_1,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.X_A_OFF_ADD_2,ctypes.c_int8(a_offset).value)

    def set_y_accel_offset(self,a_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Y_A_OFF_ADD_1,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Y_A_OFF_ADD_2,ctypes.c_int8(a_offset).value)
    
    def set_z_accel_offset(self,a_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Z_A_OFF_ADD_1,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Z_A_OFF_ADD_2,ctypes.c_int8(a_offset).value)
    
    def set_x_gyro_offset(self,g_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.X_G_OFF_ADD_1,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.X_G_OFF_ADD_2,ctypes.c_int8(g_offset).value)
    
    def set_y_gyro_offset(self,g_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Y_G_OFF_ADD_1,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Y_G_OFF_ADD_2,ctypes.c_int8(g_offset).value)
    
    def set_z_gyro_offset(self,g_offset):
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Z_G_OFF_ADD_1,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, dmp_mpu6050.Z_G_OFF_ADD_2,ctypes.c_int8(g_offset).value)


    #原始读取
    def read_raw_bits(self,register):
        high = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, register)
        low = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, register+1)
        value = (high << 8) | low
        if value > 32768:
            value -= 65536
        return value

    def write_bit(self, a_reg_add, a_bit_num, a_bit):
        byte = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, a_reg_add)
        if a_bit:
            byte |= 1 << a_bit_num
        else:
            byte &= ~(1 << a_bit_num)
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, a_reg_add, ctypes.c_int8(byte).value)


    def reset(self):
        self.write_bit(dmp_mpu6050.PWR_MGMT_1,7, 1)

    def set_sleep_enabled(self, a_enabled):
        set_bit = 0
        if a_enabled:
            set_bit = 1
        self.write_bit(dmp_mpu6050.PWR_MGMT_1,6, set_bit)

    def set_memory_bank(self, a_bank, a_prefetch_enabled=False,
                        a_user_bank=False):
        a_bank &= 0x1F
        if a_user_bank:
            a_bank |= 0x20
        if a_prefetch_enabled:
            a_bank |= 0x20
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x6D, a_bank)

    def read_bits(self, a_reg_add, a_bit_start, a_length):
        byte = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, a_reg_add)
        mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1)
        byte &= mask
        byte >>= a_bit_start - a_length + 1
        return byte

    def read_bytes(self, a_data_list, a_address, a_length):
        if a_length > len(a_data_list):
            print('read_bytes, length of passed list too short')
            return a_data_list
        # Attempt to use the built in read bytes function in the adafruit lib
        # a_data_list = self.__bus.read_i2c_block_data(self.__dev_id, a_address,
        #                                             a_length)
        # Attempt to bypass adafruit lib
        #a_data_list = self.__mpu.bus.read_i2c_block_data(0x68, a_address, a_length)
        #print('data' + str(a_data_list))
        for x in range(0, a_length):
            a_data_list[x] = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR,
                                                       a_address + x)
        return a_data_list

    def write_bits(self, a_reg_add, a_bit_start, a_length, a_data):
        byte = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, a_reg_add)
        mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1)
        # Get data in position and zero all non-important bits in data
        a_data <<= a_bit_start - a_length + 1
        a_data &= mask
        # Clear all important bits in read byte and combine with data
        byte &= ~mask
        byte = byte | a_data
        # Write the data to the I2C device
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, a_reg_add, ctypes.c_int8(byte).value)
    
    def get_x_gyro_offset_TC(self):
        return self.read_bits(0x00,6,6)

    def set_x_gyro_offset_TC(self, a_offset):
        self.write_bits(0x00,6,6, a_offset)

    def get_y_gyro_offset_TC(self):
        return self.read_bits(0x01,6,6)

    def set_y_gyro_offset_TC(self, a_offset):
        self.write_bits(0x01,6,6, a_offset)

    def get_z_gyro_offset_TC(self):
        return self.read_bits(0x02,6,6)

    def set_z_gyro_offset_TC(self, a_offset):
        self.write_bits(0x02,6,6, a_offset)

    def set_slave_address(self, a_num, a_address):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x25 + a_num * 3, a_address)

    def reset_I2C_master(self):
        self.write_bit(0x6A,1, 1)

    def set_memory_start_address(self, a_address):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x6E, a_address)
    def write_memory_block(self, a_data_list, a_data_size, a_bank, a_address,
                           a_verify):
        success = True
        self.set_memory_bank(a_bank)
        self.set_memory_start_address(a_address)

        # For each a_data_item we want to write it to the board to a certain
        # memory bank and address
        for i in range(0, a_data_size):
            # Write each data to memory
            self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, 0x6F,
                                       a_data_list[i])

            if a_verify:
                self.set_memory_bank(a_bank)
                self.set_memory_start_address(a_address)
                verify_data = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR,
                                                        0x6F)
                if verify_data != a_data_list[i]:
                    success = False

            # If we've filled the bank, change the memory bank
            if a_address == 255:
                a_address = 0
                a_bank += 1
                self.set_memory_bank(a_bank)
            else:
                a_address += 1

            # Either way update the memory address
            self.set_memory_start_address(a_address)

        return success
    
    
    
    def write_prog_memory_block(self, a_data_list, a_data_size, a_bank=0,
                                a_address=0, a_verify=True):
        return self.write_memory_block(a_data_list, a_data_size, a_bank,
                                       a_address, a_verify)

    def write_prog_dmp_configuration(self, a_data_list, a_data_size):
        return self.write_DMP_configuration_set(a_data_list, a_data_size)

    def write_DMP_configuration_set(self, a_data_list, a_data_size):
        index = 0
        while index < a_data_size:
            bank = a_data_list[index]
            offset = a_data_list[index + 1]
            length = a_data_list[index + 2]
            index += 3
        

            # Normal case
            if length > 0:
                data_selection = list()
                for subindex in range(0, length):
                    data_selection.append(a_data_list[index + subindex])
                    self.write_memory_block(data_selection, length, bank,offset, True)
                index += length
            # Special undocumented case
            else:
                special = a_data_list[index]
                index += 1
                if special == 0x01:
                    # TODO Figure out if write8 can return True/False
                   self.bus.write_byte_data(dmp_mpu6050.MPU6050_ADDR, 0x38, 0x32)

            # if success == False:
            #     # TODO implement error messagemajigger
            #     return False
            #     pass


       
        return True


    def set_clock_source(self, a_source):
        self.write_bits(dmp_mpu6050.PWR_MGMT_1, 2,
                        3, a_source)

    def set_int_enable(self, a_enabled):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x38, a_enabled)

    def set_rate(self, a_rate):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x19, a_rate)

    def set_external_frame_sync(self, a_sync):
        self.write_bits(0x1A,
                        5,
                        3, a_sync)

    def set_DLF_mode(self, a_mode):
        self.write_bits(0x1A, 2,
                        3, a_mode)

    def set_full_scale_gyro_range(self, a_data):
        self.write_bits(0x1B,
                       4,
                        2, a_data)
    def set_DMP_config_1(self, a_config):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x70, a_config)

    def get_DMP_config_2(self):
        return self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR, 0x71)
    
    def set_DMP_config_2(self, a_config):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x71, a_config)

    def set_OTP_bank_valid(self, a_enabled):
        bit = 0
        if a_enabled:
            bit = 1
        self.write_bit(0x00,
                       0, bit)

    def reset_FIFO(self):
        self.write_bit(0x6A,
                       2, True)

    def get_FIFO_count(self):
        data = [0] * 2
        data = self.read_bytes(data, 0x72, 2)
        return (data[0] << 8) | data[1]

    def get_FIFO_bytes(self, a_FIFO_count):
        return_list = list()
        for index in range(0, a_FIFO_count):
            return_list.append(
                self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR,
                                        0x74))
        return return_list

    def set_motion_detection_threshold(self, a_threshold):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x1F, a_threshold)

    def set_zero_motion_detection_threshold(self, a_threshold):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x21, a_threshold)

    def set_motion_detection_duration(self, a_duration):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x20, a_duration)

    def set_zero_motion_detection_duration(self, a_duration):
        self.bus.write_byte_data(
            dmp_mpu6050.MPU6050_ADDR, 0x22, a_duration)

    def set_FIFO_enabled(self, a_enabled):
        bit = 0
        if a_enabled:
            bit = 1
        self.write_bit(0x6A,
                       6, bit)

    def set_DMP_enabled(self, a_enabled):
        bit = 0
        if a_enabled:
            bit = 1
        self.write_bit(0x6A,
                       7, bit)

    def reset_DMP(self):
        self.write_bit(0x6A,
                       3, True)

    def get_int_status(self):
        return self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR,
                                    0x3A)

    def DMP_get_FIFO_packet_size(self):
        return self.__DMP_packet_size

    def set_I2C_master_mode_enabled(self, a_enabled):
        bit = 0
        if a_enabled:
            bit = 1
        self.write_bit(0x6A,
                       5, bit)


    def dmp_initialize(self):
        print("dmp 初始化")
        self.reset()
        time.sleep(0.1)
        #关闭休眠模式
        self.set_sleep_enabled(0)
        self.set_memory_bank(0x10, True, True)
        self.set_memory_start_address(0x6)
        time.sleep(0.1)
        #不知道干啥的
        HW_revision = self.bus.read_byte_data(dmp_mpu6050.MPU6050_ADDR,0x6F)
        self.set_memory_bank(0)
        
        #miss OTP
        x_g_offset_TC = self.get_x_gyro_offset_TC()
        y_g_offset_TC = self.get_y_gyro_offset_TC()
        z_g_offset_TC = self.get_z_gyro_offset_TC()

        self.set_slave_address(0, 0x7F)
        self.set_I2C_master_mode_enabled(False)
        self.set_slave_address(0, 0x68)
        self.reset_I2C_master()
        time.sleep(0.1)
        #加载DMP代码
        if self.write_prog_memory_block(dmp_mpu6050.dmpMemory, 1929):
            
           
            
            if self.write_prog_dmp_configuration(dmp_mpu6050.dmpConfig,192):

              
                
                self.set_clock_source(0x03)
                self.set_int_enable(0x12)
                self.set_rate(4)
                self.set_external_frame_sync(0x1)
                self.set_DLF_mode(0x03)
                self.set_full_scale_gyro_range(0x03)  #这块待定 当前设设置的最大区间 2000
                self.set_DMP_config_1(0x03)
                self.set_DMP_config_2(0x00)
                self.set_OTP_bank_valid(False)
                self.set_x_gyro_offset_TC(x_g_offset_TC)
                self.set_y_gyro_offset_TC(y_g_offset_TC)
                self.set_z_gyro_offset_TC(z_g_offset_TC)

                pos = 0
                j = 0
                dmp_update = [0] * 16
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1

                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                self.reset_FIFO()

                FIFO_count = self.get_FIFO_count()

                print('FIFO count: ' + repr(FIFO_count))
                
                FIFO_buffer = [0] * 128
                FIFO_buffer = self.get_FIFO_bytes(FIFO_count)

                self.set_motion_detection_threshold(2)
                self.set_zero_motion_detection_threshold(156)
                self.set_motion_detection_duration(80)
                self.set_zero_motion_detection_duration(0)
                self.reset_FIFO()
                self.set_FIFO_enabled(True)
                self.set_DMP_enabled(True)
                self.reset_DMP()

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)
                
                
                FIFO_count = self.get_FIFO_count()
                while FIFO_count < 3:
                    FIFO_count = self.get_FIFO_count()

                FIFO_buffer = self.get_FIFO_bytes(FIFO_count)

                MPU_int_status = self.get_int_status()

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)


                FIFO_count = self.get_FIFO_count()
                while FIFO_count < 3:
                    FIFO_count = self.get_FIFO_count()

                FIFO_buffer = self.get_FIFO_bytes(FIFO_count)
                MPU_int_status = self.get_int_status()

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                FIFO_count = self.get_FIFO_count()
                while FIFO_count < 3:
                    FIFO_count = self.get_FIFO_count()

                FIFO_buffer = self.get_FIFO_bytes(FIFO_count)

                MPU_int_status = self.get_int_status()

                j = 0
                while (j < 4) or (j < dmp_update[2] + 3):
                    dmp_update[j] = dmp_mpu6050.dmpUpdates[pos]
                    pos += 1
                    j += 1
                # Write as block from pos 3
                self.write_memory_block(dmp_update[3:], dmp_update[2],
                                        dmp_update[0], dmp_update[1], True)

                self.set_DMP_enabled(False)

                self.__DMP_packet_size = 42
                self.reset_FIFO()
                self.get_int_status()
            else:
                return 2
        else:
            return 1
        
        return 0


    def DMP_get_acceleration_int16(self, a_FIFO_buffer):
        print(len(a_FIFO_buffer))
        x = ctypes.c_int16(a_FIFO_buffer[28] << 8 | a_FIFO_buffer[29]).value
        y = ctypes.c_int16(a_FIFO_buffer[32] << 8 | a_FIFO_buffer[33]).value
        z = ctypes.c_int16(a_FIFO_buffer[36] << 8 | a_FIFO_buffer[37]).value
        return V(x, y, z)

    def DMP_get_quaternion_int16(self, a_FIFO_buffer):
        w = ctypes.c_int16((a_FIFO_buffer[0] << 8) | a_FIFO_buffer[1]).value
        x = ctypes.c_int16((a_FIFO_buffer[4] << 8) | a_FIFO_buffer[5]).value
        y = ctypes.c_int16((a_FIFO_buffer[8] << 8) | a_FIFO_buffer[9]).value
        z = ctypes.c_int16((a_FIFO_buffer[12] << 8) | a_FIFO_buffer[13]).value
        return Q(w, x, y, z)

    def DMP_get_gravity(self, a_quat):
        x = 2.0 * (a_quat.x * a_quat.z - a_quat.w * a_quat.y)
        y = 2.0 * (a_quat.w * a_quat.x + a_quat.y * a_quat.z)
        z = 1.0 * (a_quat.w * a_quat.w - a_quat.x * a_quat.x -
                   a_quat.y * a_quat.y + a_quat.z * a_quat.z)
        return V(x, y, z)


if __name__ == "__main__":
    
    mpu = dmp_mpu6050()

    mpu.dmp_initialize()
    mpu.set_DMP_enabled(True)
    mpu_int_status = mpu.get_int_status()
    # print(hex(mpu_int_status))
    # 
    packet_size = mpu.DMP_get_FIFO_packet_size()

    FIFO_count = mpu.get_FIFO_count()
  
    FIFO_buffer = [0]*64

    try:
        while True:
            try:
                FIFO_count = mpu.get_FIFO_count()
                mpu_int_status = mpu.get_int_status()
            except:
                continue
            # If overflow is detected by status or fifo count we want to reset
            if (FIFO_count == 1024) or (mpu_int_status & 0x10):
                mpu.reset_FIFO()
                print('overflow!')
            # Check if fifo data is ready
            elif (mpu_int_status & 0x02):
                # Wait until packet_size number of bytes are ready for reading, default
                # is 42 bytes
                while FIFO_count < packet_size:
                    FIFO_count = mpu.get_FIFO_count()
                FIFO_buffer = mpu.get_FIFO_bytes(packet_size)
                accel = mpu.DMP_get_acceleration_int16(FIFO_buffer)
                quat = mpu.DMP_get_quaternion_int16(FIFO_buffer)
                grav = mpu.DMP_get_gravity(quat)
                # roll_pitch_yaw = mpu.DMP_get_euler_roll_pitch_yaw(quat, grav)

                # str_show = "roll: %.2f  pitch: %.2f  yaw: %.2f        "%(roll_pitch_yaw.x,roll_pitch_yaw.y,roll_pitch_yaw.z)

                # print("\r %s"%(str_show),end='')
    except KeyboardInterrupt:
        print('\n Ctrl + C QUIT')